#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <signal.h>
#include <time.h>
#include <pthread.h>
#include <sicklms-1.0/SickLMS.hh>

#include "maestro.h"
#include "compass.h"
#include "gotemp.h"
#include "gps_boat.h"
#include "config.h"

#include <gps.h>
#include "pot_field.h"
#include "main.h"

#define REMOTE 1 


boat myBoat;

int maestro;
int compass;
FILE * logFile;
FILE * statusFile;

VECTOR origin;
double bias = 0;

bool isObs, isRotate;
double obsDistance;
VECTOR obsTempV;

extern VECTOR goal;
extern VECTOR obs;

using namespace std;
using namespace SickToolbox;

SickLMS *lmsp;

pthread_t tid;

int init(){
	fprintf(stderr, "Initializing.\n");
	signal(SIGINT, signal_handler);
	obs.lat = 0;
	obs.lon = 0;	
	maestro = maestro_connect();
	compass = compass_connect();
	fprintf(stderr, "M:%d C:%d\n", maestro, compass);

	if ((maestro < 0) || (compass < 0))
	{
		fprintf(stderr, "Maestro or compass connection error, exiting. M: %d, C: %d\n", maestro, compass);
		exit(1);
	}
        

//        pthread_create(&tid, NULL, stepperInit, NULL);

	if (gotemp_open() == -255)
	{
		return 0;
	}
	double retHeading, retPitch, retRoll, retTemp;	bool goodData;
	get_compass(&retHeading, &retPitch, &retRoll, &retTemp, &goodData);
	fprintf(stderr, "\t\tHeading: %f, Pitch: %f, Roll: %f, Temp: %f, goodData: %d\n", retHeading, retPitch, retRoll, retTemp, goodData);
	gps_connect();

//        struct obsLink *head = NULL; 
//        struct obsLink *tail = NULL;

        gps_fetch(&myBoat.current);

        myBoat.lastLat = myBoat.current.fix.latitude;
        myBoat.lastLon = myBoat.current.fix.longitude;

        if (myBoat.current.fix.latitude)
            printf("GPS NaNs....\n");

	fprintf(stderr, "\t\tDevices connected, loading configuration.\n");

	loadConfig();
	fprintf(stderr, "\t\tConfiguration loaded.\n");
	myBoat.state = -1;
	myBoat.commandStarted = false;
	char string[128];
	sprintf(string, "Outputs/log-%ld.txt", time(NULL));
	logFile = fopen(string, "w");
	return 0;
}


int main(int argc, char* argv[]){
	init();
	bool hitWaypoint = false;
        usleep(1000); 
	/* Sick sensor initialization */
        string device_str;                      
        SickLMS::sick_lms_baud_t desired_baud = SickLMS::SICK_BAUD_9600;


        device_str = "/dev/ttyUSB0";

        if (argc == 2) {
	    device_str = argv[1];
        }
        SickLMS sick_lms(device_str);
        lmsp = &sick_lms;
        /*
        * Initialize the Sick LMS 2xx
        */
        try {
            sick_lms.Initialize(desired_baud);
        }

        catch(...) {
            cerr << "Initialize failed! Are you using the correct device path?" << endl;
            return -1;
        }

	while (1)
	{
		fprintf(stderr, "\t\tBeating...\n");
		if (getCommand()){
			fprintf(stderr, "\t\t Parsing new command.\n");
							
		    	setTarget(maestro, getRudder(), 1500);
	    		usleep(1000);
    			setTarget(maestro, getThrottle(), 1500);
                        // parse new command
			parseCommand();

		}
//		fprintf(stderr, "Compass.\n");
		get_compass(&myBoat.retHeading, &myBoat.retPitch, &myBoat.retRoll, &myBoat.retTemp, &myBoat.goodData);
		myBoat.retHeading-=80;
//		fprintf(stderr, "GPS.\n");
		gps_fetch(&myBoat.current);
 		
//		myBoat.current.fix.latitude = 40.436759;
//        	myBoat.current.fix.longitude = -79.948484;


               
                fetchSick();

		myBoat.tempProbe = gotemp_read();
//		fprintf(stdemyBoat.current.fix.latitude,rr, "Log.\n");
		if ((time(NULL) % secsPerLog) == 0){
			
			printLog();
		}
		printStatus();
		switch(myBoat.state){
			case WAYPOINT: {
				
				if (myBoat.commandStarted == false) {fprintf(logFile, "<waypoint>"); myBoat.commandStarted = true;}
				
				// Check GPS validity
				if (myBoat.current.status == 0 || ((myBoat.current.fix.latitude == 180.0) && (myBoat.current.fix.longitude == 0.0))){
//					fprintf(stderr, "Grabbing GPS fix");
					break;
				}

				// Secondary check to ensure that we have real distances
				double distance = calcDistance(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);
				if (isnan(distance)){	
//					fprintf(stderr, "Still isnan distance.\n");	
					break;
				}

				if (distance < myBoat.commandArgs[2])
				{
//					fprintf(stderr, "At waypoint.\n");
					fprintf(logFile, "<comment>At waypoint.</comment>\n</wayPoint>\n");
					myBoat.commandStarted = false;
					myBoat.state = -1;
					break;
				}
				//double diffLat =  myBoat.current.fix.latitude - myBoat.commandArgs[0];
				//double diffLon =  myBoat.current.fix.longitude - myBoat.commandArgs[1];
				//fprintf(stderr, "Diff lat: %lf, diff lon: %lf\n", diffLat, diffLon);
				double desiredHeading = calcHeading(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);

				double headingDiff = desiredHeading - myBoat.retHeading;
				if (headingDiff < -180)
					headingDiff = headingDiff + 360;
				if (headingDiff > 180)
					headingDiff = headingDiff - 360;

				double commandRudder = 1500 - myBoat.commandArgs[3]*getRudderCoeff()*headingDiff;

//				fprintf(stderr, "Math: %lf. coeff: %lf, headingDff: %lf, Commanded rudder: %d.\n", myBoat.commandArgs[3], getRudderCoeff(), headingDiff, commandRudder);
				
				if (commandRudder >= 2000)
					commandRudder = 2000;
				else if (commandRudder <= 1000)
					commandRudder = 1000;
				unsigned int commandRudderui = static_cast<unsigned int>(commandRudder);

				double commandThrottle = 1500 - myBoat.commandArgs[3]*getThrottleCoeff()*distance;

				fprintf(stderr, "retHeading: %lf\n", myBoat.retHeading);
				if (commandThrottle >= 1800)
					commandThrottle = 1800;
				else if (commandThrottle <= 1200)
					commandThrottle = 1200;
				unsigned int commandT = static_cast<unsigned int>(commandThrottle);	
				
				setTarget(maestro, getRudder(), commandRudderui);
				usleep(1000);
				setTarget(maestro, getThrottle(), commandT);
	

				fprintf(stderr, "Distance to waypoint: %lf. Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.\n", distance, headingDiff, commandT, commandRudderui);
				fprintf(logFile, "<comment>Distance to waypoint: %lf. Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.</comment>\n", distance, headingDiff, commandT, commandRudderui);
				}
				break;

			case HEADING: {
			
				if (myBoat.commandStarted == false) {fprintf(logFile, "<heading>"); myBoat.commandStarted = true;}
				double desiredHeading = myBoat.commandArgs[0];
				double headingDiff = desiredHeading - myBoat.retHeading;
				if (headingDiff < -180)
					headingDiff = headingDiff + 360;
				if (headingDiff > 180)
					headingDiff = headingDiff - 360;

				unsigned int commandRudder = 1500 - getRudderCoeff()*headingDiff;
				if (commandRudder > 2000)
					commandRudder = 2000;
				else if (commandRudder < 1000)
					commandRudder = 1000;
				
				setTarget(maestro, getRudder(), commandRudder);

				if (headingDiff < 5)
				{
					setTarget(maestro, getRudder(), 1500);
					myBoat.commandStarted = false;
					fprintf(logFile, "</heading>");
					myBoat.state = -1;
				}

				}
				break;

			case HALT: {
				if (myBoat.commandStarted == false) {fprintf(logFile, "<halt>"); myBoat.commandStarted = true;}
				setTarget(maestro, getRudder(), 1500);
				setTarget(maestro, getThrottle(), 1500);

				}
				break;
			case ATSPEED: {
				
				if (myBoat.commandStarted == false) {fprintf(logFile, "<atspeed>"); myBoat.commandStarted = true;}
				
				// Check GPS validity
				if (myBoat.current.status == 0 || ((myBoat.current.fix.latitude == 180.0) && (myBoat.current.fix.longitude == 0.0))){
					fprintf(stderr, "Grabbing GPS fix");
					break;
				}

				// Secondary check to ensure that we have real distances
				double distance = calcDistance(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);
				if (isnan(distance)){	
					fprintf(stderr, "Still isnan distance.\n");	
					break;
				}

				//double diffLat =  myBoat.current.fix.latitude - myBoat.commandArgs[0];
				//double diffLon =  myBoat.current.fix.longitude - myBoat.commandArgs[1];
				//fprintf(stderr, "Diff lat: %lf, diff lon: %lf\n", diffLat, diffLon);
				double desiredHeading = calcHeading(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);

				double headingDiff = desiredHeading - myBoat.retHeading;
				if (headingDiff < -180)
					headingDiff = headingDiff + 360;
				if (headingDiff > 180)
					headingDiff = headingDiff - 360;

				unsigned int commandRudder = 1500 - getRudderCoeff()*headingDiff;
				if (commandRudder >= 2000)
					commandRudder = 2000;
				else if (commandRudder <= 1000)
					commandRudder = 1000;


				unsigned int commandThrottle = 1500 + myBoat.commandArgs[2];
				if (commandThrottle >= 2000)
					commandThrottle = 2000;
				else if (commandThrottle <= 1000)
					commandThrottle = 1000;
				
				setTarget(maestro, getRudder(), commandRudder);
				usleep(1000);
				setTarget(maestro, getThrottle(), commandThrottle);
	

				fprintf(stderr, "Distance to waypoint: %lf. Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.\n", distance, headingDiff, commandThrottle, commandRudder);
				fprintf(logFile, "<comment>Distance to waypoint: %lf. Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.</comment>\n", distance, headingDiff, commandThrottle, commandRudder);
				}
				break;


			case ATHEADINGSPEED: {
				// myBoat.commandArgs[0] is heading, commadArgs[1] is speed
				if (myBoat.commandStarted == false) {fprintf(logFile, "<atheadingspeed>"); myBoat.commandStarted = true;}
				
				
				// Check GPS validity
				/*if (myBoat.current.status == 0 || ((myBoat.current.fix.latitude == 180.0) && (myBoat.current.fix.longitude == 0.0))){
					fprintf(stderr, "Grabbing GPS fix");
					break;
				}
				
				// Secondary check to ensure that we have real distances
				double distance = calcDistance(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);
				if (isnan(distance)){	
					fprintf(stderr, "Still isnan distance.\n");	
					break;
				}

				if (distance < myBoat.commandArgs[2])
				{
					fprintf(stderr, "At waypoint.\n");
					fprintf(logFile, "<comment>At waypoint.</comment>\n</wayPoint>\n");
					myBoat.commandStarted = false;
					myBoat.state = -1;
					break;
				}
				*/

				//double diffLat =  myBoat.current.fix.latitude - myBoat.commandArgs[0];
				//double diffLon =  myBoat.current.fix.longitude - myBoat.commandArgs[1];
				//fprintf(stderr, "Diff lat: %lf, diff lon: %lf\n", diffLat, diffLon);
				double desiredHeading = myBoat.commandArgs[0]; //calcHeading(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);

				double headingDiff = desiredHeading - myBoat.retHeading;
				if (headingDiff < -180)
					headingDiff = headingDiff + 360;
				if (headingDiff > 180)
					headingDiff = headingDiff - 360;

				unsigned int commandRudder = 1500 - getRudderCoeff()*headingDiff;
				if (commandRudder >= 2000)
					commandRudder = 2000;
				else if (commandRudder <= 1000)
					commandRudder = 1000;

				
				unsigned int commandThrottle = 1500 - myBoat.commandArgs[1];
				if (fabs(headingDiff) > 5) {
					if (commandThrottle >= 1700)
						commandThrottle = 1700;
					else if (commandThrottle <= 1300)
						commandThrottle = 1300;
				}				
				else {				
					if (commandThrottle >= 2000)
						commandThrottle = 2000;
					else if (commandThrottle <= 1000)
						commandThrottle = 1000;
				}
				setTarget(maestro, getRudder(), commandRudder);
				usleep(1000);
				setTarget(maestro, getThrottle(), commandThrottle);
	

				fprintf(stderr, "Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.\n", headingDiff, commandThrottle, commandRudder);
				fprintf(logFile, "<comment> Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.</comment>\n",  headingDiff, commandThrottle, commandRudder);
				}
				break;

			case ATHEADINGSPEEDMIXED: {
				// myBoat.commandArgs[0] is heading, commadArgs[1] is speed
				if (myBoat.commandStarted == false) {fprintf(logFile, "<atheadingspeedmixed>"); myBoat.commandStarted = true;}
				
				
				// Check GPS validity
				/*if (myBoat.current.status == 0 || ((myBoat.current.fix.latitude == 180.0) && (myBoat.current.fix.longitude == 0.0))){
					fprintf(stderr, "Grabbing GPS fix");
					break;
				}
				
				// Secondary check to ensure that we have real distances
				double distance = calcDistance(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);
				if (isnan(distance)){	
					fprintf(stderr, "Still isnan distance.\n");	
					break;
				}

				if (distance < myBoat.commandArgs[2])
				{
					fprintf(stderr, "At waypoint.\n");
					fprintf(logFile, "<comment>At waypoint.</comment>\n</wayPoint>\n");
					myBoat.commandStarted = false;
					myBoat.state = -1;
					break;
				}
				*/

				//double diffLat =  myBoat.current.fix.latitude - myBoat.commandArgs[0];
				//double diffLon =  myBoat.current.fix.longitude - myBoat.commandArgs[1];
				//fprintf(stderr, "Diff lat: %lf, diff lon: %lf\n", diffLat, diffLon);
				double desiredHeading = myBoat.commandArgs[0]; //calcHeading(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);

				double headingDiff = desiredHeading - myBoat.retHeading;
				if (headingDiff < -180)
					headingDiff = headingDiff + 360;
				if (headingDiff > 180)
					headingDiff = headingDiff - 360;

				unsigned int commandRudder = 1500 - getRudderCoeff()*headingDiff;
				if (commandRudder >= 2000)
					commandRudder = 2000;
				else if (commandRudder <= 1000)
					commandRudder = 1000;


				unsigned int commandThrottle = 1500 + myBoat.commandArgs[1];
				if (commandThrottle >= 2000)
					commandThrottle = 2000;
				else if (commandThrottle <= 1000)
					commandThrottle = 1000;
				
				// if (abs(ruddur-neutral) + abs(throttle-neutral) > 750)
				double rudderScale = abs(commandRudder - 1500) / 500;
				
				commandThrottle = (1-rudderScale) * (commandThrottle - 1500) + 1500;

				setTarget(maestro, getRudder(), commandRudder);
				usleep(1000);
				setTarget(maestro, getThrottle(), commandThrottle);
	

				fprintf(stderr, "Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.\n", headingDiff, commandThrottle, commandRudder);
				fprintf(logFile, "<comment> Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.</comment>\n",  headingDiff, commandThrottle, commandRudder);
				}
				break;

                        case ATOBSAVOIDNAIVE:
			case ATOBSAVOID: {
                               
                                if (isnan(myBoat.current.fix.latitude)) {
                                    printf("GPS value false!\n");
                                    break;
                                }

                                double desiredHeading = 0;
				double distance = lawCosines(myBoat.current.fix.latitude, myBoat.current.fix.longitude, goal.lat, goal.lon);
				unsigned int commandRudder, commandThrottle;
                                fprintf(stderr,"boat[%lf, %lf], goal[%lf, %lf]\n", myBoat.current.fix.latitude, myBoat.current.fix.longitude, goal.lat, goal.lon);
                                fprintf(stderr, "Distance to goal: %lf.\n", distance);

				if (distance < RADIUS_GOAL) {
					fprintf(stderr, "Goal reached!!!\n");
					fprintf(logFile, "<comment>At obstacle avoidance.</comment>\n</obstacle avoidance>\n");
					myBoat.commandStarted = false;
					myBoat.state = -1;
					setTarget(maestro, getRudder(), 1500);
					usleep(1000);
					setTarget(maestro, getThrottle(), 1500);
					break;
				}
				
                                if (myBoat.commandStarted == false) {fprintf(logFile, "<atobsavoid>"); myBoat.commandStarted = true;}
				

                                desiredHeading = calcHeading(myBoat.current.fix.latitude, myBoat.current.fix.longitude, goal.lat, goal.lon);
                                
                                printf("retHeading: %lf, Desired Heading: %lf, \n", myBoat.retHeading, desiredHeading);
                               myBoat.commandArgs[0] =desiredHeading;
                                double headingDiff = desiredHeading - myBoat.retHeading;

				fprintf(stderr, "Heading difference: %lf\n", headingDiff);
                                if (headingDiff < -180)
				    headingDiff = headingDiff + 360;
				if (headingDiff > 180)
				    headingDiff = headingDiff - 360;
				
                                if ((headingDiff > 45) || (headingDiff < -45))
                                {
				    
				    printf("Rotating....\n");
				    commandRudder = 1500 - getRudderCoeff()*headingDiff;
				    if (commandRudder >= 2000)
					commandRudder = 2000;
				    else if (commandRudder <= 1000)
					commandRudder = 1000;

                                    commandThrottle = 1500;
                                }
                                else if (fabs(headingDiff) < 20)
                                {
        			    printf("Full speed...\n");
                                    commandRudder = 1500 - getRudderCoeff()*headingDiff;
                                    commandThrottle = 1000;
                                }
                                else
                                {
        			    printf("Adjust...\n");
                                    commandRudder = 1500 - getRudderCoeff()*headingDiff;
                                    commandThrottle = 1200;
                                }
				    
				fprintf(stderr, "Commanded rudder: %d, Commanded throttle: %d\n", commandRudder, commandThrottle);
                                setTarget(maestro, getRudder(), commandRudder);
				usleep(1000);
				setTarget(maestro, getThrottle(), commandThrottle);
	

				}
				break;

                        case ATSTEPPER: {

                                pthread_create(&tid, NULL, stepperRotate, NULL);
				myBoat.state = -1;
				}
                                break;
			default: {
				setTarget(maestro, getRudder(), 1500);
				usleep(1000);
				setTarget(maestro, getThrottle(), 1500);

				}
				break;

	
		}
                myBoat.lastLat = myBoat.current.fix.latitude;
                myBoat.lastLon = myBoat.current.fix.longitude;
	}

}

void printLog()
{
//	fprintf(stderr, "Printing Log.\n");

	fprintf(logFile, "<status>\n");
	time_t rawtime;
	struct tm * timeinfo;
	time ( &rawtime );
	timeinfo = localtime ( &rawtime );
	fprintf(logFile, "\t<compTime>%s</compTime>\n", asctime(timeinfo));
	
	time_t unixTime = myBoat.current.fix.time;
	fprintf(logFile, "\t<gpsTime>%s</gpsTime>\n", asctime(localtime(&unixTime)));
	double obsHeading;

	
	switch(myBoat.state)
	{		
		case WAYPOINT:
			fprintf(logFile, "\t<state>Waypoint</state>\n");
			fprintf(logFile, "\t\t<commandLat>%lf</commandLat>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<commandLon>%lf</commandLon>\n", myBoat.commandArgs[1]);
			fprintf(logFile, "\t\t<commandPrecision>%lf</commandPrecision>\n", myBoat.commandArgs[2]);
			fprintf(logFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[3]); 
			break;
		case HEADING:
			fprintf(logFile, "\t<state>Heading</state>\n");
			fprintf(logFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[1]);
			break;
		case HALT:
			fprintf(logFile, "\t<state>Halt</state>\n");
			break;
		case ATSPEED:
			fprintf(logFile, "\t<state>At Speed</state>\n");
			fprintf(logFile, "\t\t<commandLat>%lf</commandLat>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<commandLon>%lf</commandLon>\n", myBoat.commandArgs[1]);
			fprintf(logFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[2]); 
			break;
		case ATHEADINGSPEED:
			fprintf(logFile, "\t<state>At Heading and Speed</state>\n");
			fprintf(logFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[1]);
			break;
		case ATHEADINGSPEEDMIXED:
			fprintf(logFile, "\t<state>At Heading and Speed, mixed</state>\n");
			fprintf(logFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[1]);
			break;
                case ATOBSAVOIDNAIVE:
                case ATOBSAVOID:
        		fprintf(logFile, "\t<state>At Obstacle Avoidance</state>\n");
			fprintf(logFile, "\t\t<goalLat>%lf</goalLat>\n", goal.lat);
			fprintf(logFile, "\t\t<goalLon>%lf</goalLon>\n", goal.lon);

                        fprintf(logFile, "\t\t<desiredHeading>%lf</desiredHeading>\n", myBoat.commandArgs[0]);
                        break;
	        case ATSTEPPER:
        		fprintf(logFile, "\t<state>At Stepper</state>\n");
			fprintf(logFile, "\t\t<direction>%lf</direction>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<steps>%lf</steps>\n", myBoat.commandArgs[1]);
                        break;	
                default:
			fprintf(logFile, "\t<state>Waiting</state>\n");
			break;
	}
	switch(myBoat.commandStarted)
	{
		case true: fprintf(logFile, "\t<started>1</started>\n"); break;
		case false: fprintf(logFile, "\t<started>0</started>\n"); break;
	}
	fprintf(logFile, "\t<lat>%lf</lat>\n", myBoat.current.fix.latitude);
	fprintf(logFile, "\t<lon>%lf</lon>\n", myBoat.current.fix.longitude);
	fprintf(logFile, "\t<compass>%lf</compass>\n", myBoat.retHeading);
	fprintf(logFile, "\t<temp>%lf</temp>\n", myBoat.tempProbe);
        if (!REMOTE)
        {
            obsDistance = sqrt(obs.lon * obs.lon + obs.lat * obs.lat);
	    if (obsDistance == 0)
	    {
                fprintf(logFile, "\t\t<obsLat>%lf</obsLat>\n", 0.0);
                fprintf(logFile, "\t\t<obsLon>%lf</obsLon>\n", 0.0);
 	    }
	    else
	    {	
	        obsHeading = myBoat.retHeading * M_PI / 180 + atan(obs.lon / obs.lat);
	        newPoint(myBoat.current.fix.latitude, myBoat.current.fix.longitude, obsHeading, obsDistance, &obsTempV);
	        fprintf(logFile, "\t\t<obsLat>%lf</obsLat>\n", obsTempV.lat);
                fprintf(logFile, "\t\t<obsLon>%lf</obsLon>\n", obsTempV.lon);
	    }
        }
        else
        {
/*
            fprintf(logFile, "\t\t<obsCounter>%d</obsCounter>\n", obsCounter);
            fprintf(logFile, "\t\t<obs array>\n");
            for (unsigned int i = 0; i < obsCounter; i++)
            {
                fprintf(logFile, "\t\t<s%d>%d</s%d>\n", i, obsA[i].startIndex, i);
                fprintf(logFile, "\t\t<sv%d>%d</sv%d>\n", i, obsA[i].startValue, i);
                fprintf(logFile, "\t\t<e%d>%d</e%d>\n", i, obsA[i].endIndex, i);
                fprintf(logFile, "\t\t<ev%d>%d</ev%d>\n", i, obsA[i].endValue, i);
            }
*/
        }
	fprintf(logFile, "</status>\n");

//	fprintf(stderr, "Wrote to log.\n");
}

void printStatus()
{
//	fprintf(stderr, "Printing status.\n");

	
	statusFile = fopen("Outputs/status.txt", "w");	
	fprintf(statusFile, "<status>\n");
	time_t rawtime;
	struct tm * timeinfo;
	time ( &rawtime );
	timeinfo = localtime ( &rawtime );
	fprintf(statusFile, "\t<compTime>%s</compTime>\n", asctime(timeinfo));

	time_t unixTime = myBoat.current.fix.time;
	fprintf(statusFile, "\t<gpsTime>%s</gpsTime>\n", asctime(localtime(&unixTime)));

	switch(myBoat.state)
	{		
		case WAYPOINT:
			fprintf(statusFile, "\t<state>Waypoint</state>\n");
			fprintf(statusFile, "\t\t<commandLat>%lf</commandLat>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<commandLon>%lf</commandLon>\n", myBoat.commandArgs[1]);
			fprintf(statusFile, "\t\t<commandPrecision>%lf</commandPrecision>\n", myBoat.commandArgs[2]);
			fprintf(statusFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[3]); 
			break;
		case HEADING:
			fprintf(statusFile, "\t<state>Heading</state>\n");
			fprintf(statusFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[1]);
			break;
		case HALT:
			fprintf(statusFile, "\t<state>Halt</state>\n");
			break;
		case ATSPEED:
			fprintf(statusFile, "\t<state>At Speed</state>\n");
			fprintf(statusFile, "\t\t<commandLat>%lf</commandLat>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<commandLon>%lf</commandLon>\n", myBoat.commandArgs[1]);
			fprintf(statusFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[2]); 
			break;
		case ATHEADINGSPEED:
			fprintf(statusFile, "\t<state>At Heading and Speed</state>\n");
			fprintf(statusFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[1]);
			break;
		case ATHEADINGSPEEDMIXED:
			fprintf(statusFile, "\t<state>At Heading and Speed, mixed</state>\n");
			fprintf(statusFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[1]);
                        break;
		case ATOBSAVOIDNAIVE:
                case ATOBSAVOID:
			fprintf(statusFile, "\t<state>At Obstacle Avoidance</state>\n");
			fprintf(statusFile, "\t\t<goalLat>%lf</goalLat>\n", goal.lat);
                        fprintf(statusFile, "\t\t<goalLon>%lf</goalLon>\n", goal.lon);
			fprintf(statusFile, "\t\t<desiredHeading>%lf</desiredHeading>\n", myBoat.commandArgs[0]);
                        break;
		case ATSTEPPER:
			fprintf(statusFile, "\t<state>At Stepper</state>\n");
			fprintf(statusFile, "\t\t<direction>%lf</direction>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<steps>%lf</steps>\n", myBoat.commandArgs[1]);
                        break;
		default:
			break;
	}
	switch(myBoat.commandStarted)
	{
		case true: fprintf(statusFile, "\t<started>1</started>\n"); break;
		case false: fprintf(statusFile, "\t<started>0</started>\n"); break;
	}
	fprintf(statusFile, "\t<lat>%lf</lat>\n", myBoat.current.fix.latitude);
	fprintf(statusFile, "\t<lon>%lf</lon>\n", myBoat.current.fix.longitude);
	fprintf(statusFile, "\t<compass>%lf</compass>\n", myBoat.retHeading);
	fprintf(statusFile, "\t<temp>%lf</temp>\n", myBoat.tempProbe);
        if (!REMOTE)
        {
	    if (obsDistance == 0)
	    {
                fprintf(statusFile, "\t<obsLat>%lf</obsLat>\n", 0.0);
                fprintf(statusFile, "\t<obsLon>%lf</obsLon>\n", 0.0);
 	    }
	    else
	    {	
	        fprintf(statusFile, "\t<obsLat>%lf</obsLat>\n", obsTempV.lat);
                fprintf(statusFile, "\t<obsLon>%lf</obsLon>\n", obsTempV.lon);
            }
        }
        else
        {
/*
            fprintf(statusFile, "\t<obsCounter>%d</obsCounter>\n", obsCounter);
            fprintf(statusFile, "\t<obs array>\n");
            for (unsigned int i = 0; i < obsCounter; i++)
            {
                fprintf(statusFile, "\t<s%d>%d</s%d>\n", i, obsA[i].startIndex, i);
                fprintf(statusFile, "\t<sv%d>%d</sv%d>\n", i, obsA[i].startValue, i);
                fprintf(statusFile, "\t<e%d>%d</e%d>\n", i, obsA[i].endIndex, i);
                fprintf(statusFile, "\t<ev%d>%d</ev%d>\n", i, obsA[i].endValue, i);
            }
*/
        }
	
        fprintf(statusFile, "</status>\n");
	fclose(statusFile);
//	fprintf(stderr, "Finished writing status.\n");
}

int getCommand()
{
//	fprintf(stderr, "Get Command.\n");
	FILE* fp = fopen("Commands/mission.txt", "r");
	if (fp == NULL)	{
		return 0;
	}	
	else {
		fclose(fp);
		myBoat.commandStarted = false;
		// Cleanup
		switch(myBoat.state){
			case WAYPOINT:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</waypoint>");
				break;
			case HEADING:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</heading>");
				break;
			case HALT:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</halt>");
				break;
			case ATSPEED:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</atspeed>");
				break;
			case ATHEADINGSPEED:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</atheadingspeed>");
				break;
			case ATHEADINGSPEEDMIXED:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</atheadingspeedmixed>");
				break;
			case ATOBSAVOIDNAIVE:
                                fprintf(logFile, "<comment>Interrupted.</comment>\n</atobsavoidnaive>");
                                break;
                        case ATOBSAVOID:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</atobsavoid>");
				break;
			case ATSTEPPER:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</atstepper>");
				break;
			default:
				break;
		}
		return 1;
	}
}	

int parseCommand()
{
	fprintf(stderr, "Parsing command.\n");
	rename( "Commands/mission.txt", "Commands/mission-parsing.txt");
	FILE* fp = fopen("Commands/mission-parsing.txt", "r");
	char str[MAX_LINE];
	while ((fgets(str, MAX_LINE, fp) != NULL))
	{
		char *pch;
		pch = strchr(str, '#');
		if (pch != NULL)
		{
			//fprintf(stderr, "Comment: %s", str);
		}
		else
		{
			int empty = strcmp(str, "\n");
			if (empty == 1)
			{
				char function[50];
				char args[50];

				sscanf(str, "%s \t %[^\n] \n", function, args);
				
				fprintf(stderr, "%s\n", args);
				if (!strcmp(function, "goToWaypoint"))
				{
					double lat = 1;
					double lon = 1;
					double precision = 1;// lon, precision;
					double speed = 1;
					sscanf(args, "[ %lf, %lf, %lf, %lf ]", &lat, &lon, &precision, &speed);
					//fprintf(stderr, "%f\n", lat);
					//fprintf(stderr, "Going to waypoint: %f,%f with %f precision, %f speed\n", lat, lon, precision, speed);
					
					myBoat.state = WAYPOINT;
					myBoat.commandArgs[0] = lat;
					myBoat.commandArgs[1] = lon;
					myBoat.commandArgs[2] = precision;
					myBoat.commandArgs[3] = speed;
					printLog();
					
				}
				else if (!strcmp(function, "goAtSpeed"))
				{
					double lat = 1;
					double lon = 1;
					double speed = 1;
					sscanf(args, "[ %lf, %lf, %lf ]", &lat, &lon, &speed);
					myBoat.state = ATSPEED;
					myBoat.commandArgs[0] = lat;
					myBoat.commandArgs[1] = lon;
					myBoat.commandArgs[2] = speed;
					printLog();
					
				}

				else if (!strcmp(function, "goHeadingSpeed"))
				{
					double heading = 0;
					double speed = 1;
					sscanf(args, "[ %lf, %lf]", &heading, &speed);
					myBoat.state = ATHEADINGSPEED;
					myBoat.commandArgs[0] = heading;
					myBoat.commandArgs[1] = speed;
					printLog();
					
				}

				else if (!strcmp(function, "goHeadingSpeedMixed"))
				{
					double heading = 0;
					double speed = 1;
					sscanf(args, "[ %lf, %lf]", &heading, &speed);
					myBoat.state = ATHEADINGSPEEDMIXED;
					myBoat.commandArgs[0] = heading;
					myBoat.commandArgs[1] = speed;
					printLog();
					
				}
				else if(!strcmp(function, "goToHeading"))
				{
					double desHeading;
					double speed = 1;
					sscanf(args, "[%lf, %lf]", &desHeading, &speed);
					//fprintf(stderr, "Going to desired heading of: %f\n", desHeading);

					myBoat.state = HEADING;
					myBoat.commandArgs[0] = desHeading;
					myBoat.commandArgs[1] = speed;
					printLog();
				}
				else if(!strcmp(function, "goObsAvoidNaive"))
				{
					sscanf(args, "[%lf, %lf]", &goal.lat, &goal.lon);

					myBoat.state = ATOBSAVOIDNAIVE;
//					printLog();
				}
				else if(!strcmp(function, "goObsAvoidHeading"))
				{
					double desHeading;
                                        double distance = 1;
                                        sscanf(args, "[%lf, %lf]", &desHeading, &distance);
                                        
                                        desHeading = desHeading * M_PI / 180;
                                        goal.lat = cos(desHeading) * distance + myBoat.current.fix.latitude;
                                        goal.lon = sin(desHeading) * distance + myBoat.current.fix.longitude;

					myBoat.state = ATOBSAVOID;
					printLog();
				}
                                else if(!strcmp(function, "goObsAvoid"))
				{
					sscanf(args, "[%lf, %lf]", &goal.lon, &goal.lat);
                                        if ((goal.lon == 0) && (goal.lat == 0))
                                        {
                                            goal.lon = origin.lon;
                                            goal.lat = origin.lat;
                                        }
                                        else
                                        {
                                            double distance = sqrt(goal.lat * goal.lat + goal.lon * goal.lon);
                                            double bearing = atan(goal.lat / goal.lon);

                                            if (goal.lon < 0)
                                                bearing += M_PI;

                                            bearing = bias * M_PI / 180 - bearing;
					    fprintf(stderr, "bearing: %lf, distance: %lf\n", bearing, distance);
				            if (bearing < 0)
					        bearing = bearing + 2 * M_PI;
				            else if (bearing > 2 * M_PI)
					        bearing = bearing - 2 * M_PI;

                                            newPoint(origin.lat, origin.lon, bearing, distance, &goal);
					}

                                        myBoat.state = ATOBSAVOID;
					printLog();
				}
				else if(!strcmp(function, "stepper"))
				{
					int direction;
                                        double steps;
                                        sscanf(args, "[%d, %lf]", &direction, &steps);
                                        direction *= 2000;
                                        steps *= 3;

					myBoat.state = ATSTEPPER;
					myBoat.commandArgs[0] = direction;
					myBoat.commandArgs[1] = steps;
					printLog();
				}
				else if(!strcmp(function, "halt"))
				{
					myBoat.state = HALT;
					printLog();
				}
				else if (!strcmp(function, "setFrame"))
				{
					double latX = 1;
					double lonX = 1;
					sscanf(args, "[%lf, %lf, %lf, %lf]", &origin.lat, &origin.lon, &latX, &lonX);
					bias = calcHeading(origin.lat, origin.lon, latX, lonX);
    			                fprintf(logFile, "\t<state>Setframe</state>\n");
			                fprintf(logFile, "\t\t<origin>%lf, %lf</origin>\n", origin.lat, origin.lon);
	                                fprintf(stderr, "\t\t bias: %lf\n", bias); 
					fprintf(stderr, "\t\t New frame set.\n");			
                                }
				else
				{
					fprintf(stderr, "cannot parse %s\n", function);
				}

			}
			else
			{
				//fprintf(stderr, "Empty line\n");
			}
		}
	}

	fclose(fp);
	rename( "Commands/mission-parsing.txt", "Commands/mission-running.txt");
	return 0;
}

double calcDistance(double lat1, double lon1, double lat2, double lon2)
{
	// Implemented using the haversine formula
	double radius = 6371000;	

	lat1 = lat1 * M_PI/180;
	lat2 = lat2 * M_PI/180;
	lon1 = lon1 * M_PI/180;
	lon2 = lon2 * M_PI/180;
	double dLat = (lat2-lat1);
	double dLon = (lon2-lon1);
		
	double a = sin(dLat/2) * sin(dLat/2) +
		 sin(dLon/2) * sin(dLon/2) * cos(lat1) * cos(lat2);
	double c = 2 * atan2(sqrt(a), sqrt(1-a));


	return (c * radius);
}

double calcHeading(double lat1, double lon1, double lat2, double lon2)
{
	lat1 = lat1 * M_PI/180;
	lat2 = lat2 * M_PI/180;
	lon1 = lon1 * M_PI/180;
	lon2 = lon2 * M_PI/180;
	double dLon = lon2-lon1;
	//double dLat = lat2-lat1;

	double y = sin(dLon) * cos(lat2);
	double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
	double desiredHeading = (atan2(y, x) * 180/M_PI);
	return fmod(desiredHeading + 360.0, 360);
}

void fetchSick() {
    unsigned int values[SickLMS::SICK_MAX_NUM_MEASUREMENTS] = {0}; // Uses macro defined in SickLMS.hh
    unsigned int num_values = 0;                                   // Holds the number of measurements returned
    int tempMin = 0;
    double tempI = 45; 
    int Xtemp = 0;
    int Ytemp = 0;
    FILE *obsFile;

        /*
         * Acquire a few scans from the Sick LMS
         */
        try {


            lmsp->GetSickScan(values,num_values);                   //values: distance; num_values: number of scan


            tempMin = values[0];
            
            if (!REMOTE)                                             //plan onboard
            {
                for (unsigned int i=1; i<num_values; i++)
                {
                    if((int)values[i] + TOLERANCE< tempMin)
                    {
                        tempMin = values[i];
                        tempI = 90.0 / (num_values - 1) * i+45;
                    }
                }
            
                Xtemp = cos((tempI*M_PI)/180)*tempMin;
                Ytemp = sin((tempI*M_PI)/180)*tempMin;

                if (abs(Ytemp) > LENGTH)
                {
                    obs.lat = 0;
                    obs.lon = 0;
                    isObs = false;
                    isRotate = false;
                }
                else if (!isObs)
                {
                    if (abs(Xtemp) < WIDTH)
                    {
                        obs.lon = Xtemp/100.0;
                        obs.lat = Ytemp/100.0;
                        isObs = true;
                        isRotate = true;
                    }
                    else if (abs(Xtemp) < SPREAD)
                    {
                        obs.lon = Xtemp/100.0;
                        obs.lat = Ytemp/100.0;
                        isObs = true;
                    }
                    else
                    {
                        obs.lon = 0;
                        obs.lat = 0;
                    }
                }
                else if(isRotate)
                {
                    if (abs(Xtemp > SPREAD))
                        isRotate = false;
                    else
                    {
                        obs.lon = Xtemp/100.0;
                        obs.lat = Ytemp/100.0;
                    }
                }
                else
                {
                    if (abs(Xtemp) > SPREAD + 500)
                    {
                        obs.lon = 0;
                        obs.lat = 0;
                        isObs = false;
                        isRotate = false;
                    }
                    else if (abs(Xtemp) < WIDTH)
                    {
                        obs.lon = Xtemp/100.0;
                        obs.lat = Ytemp/100.0;
                        isRotate = true;
                    }
                    else
                    {
                        obs.lon = Xtemp/100.0;
                        obs.lat = Ytemp/100.0;
                    }
                }


                if (isObs)
                    cout << "\t X:"<< obs.lon <<"\t Y:"<< obs.lat <<"\t Heading:"<< tempI << endl;  
            }
            else                                                        //plan on remote computer
            {
            }
                obsFile=fopen("/home/actuv/boats code/New Boats Code/Outputs/obs.txt","w+");

                for (unsigned int i=0; i<num_values; i++)
                {
//                    if (values[i] < LENGTH)
		    { 
/*
                        tempI = 90.0 / (num_values - 1) * i+45; 
                        obsBearing = myBoat.retHeading + 90 - tempI;
                        Xtemp = cos((tempI*M_PI)/180)*values[i];
                        Ytemp = sin((tempI*M_PI)/180)*values[i];
                        fprintf(obsFile, "\t\t<vLon%d>%d</vLon%d>\n", i, Xtemp, i);
                        fprintf(obsFile, "\t\t<vLat%d>%d</vLat%d>\n", i, Ytemp, i);
*/
                        fprintf(obsFile, "\t\t<value%d>%d</value%d>\n", i, values[i], i);
                    }
                
		}
                fclose(obsFile);
            
        }
        /* Catch anything else and exit */ 
        catch(...) {
            cerr << "An error occurred!" << endl;
        }

}

void signal_handler(int signum)
{
	printf("Ctrl + C Caught!\n");
	setTarget(maestro, getRudder(), 1500);
	setTarget(maestro, getThrottle(), 1500);
	switch(myBoat.state){
		case WAYPOINT:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</waypoint>");
			fclose(logFile);
			break;
		case HEADING:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</heading>");
			fclose(logFile);
			break;
		case HALT:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</halt>");
			fclose(logFile);			
			break;
		case ATSPEED:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atspeed>");
			fclose(logFile);			
			break;
		case ATHEADINGSPEED:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atheadingspeed>");
			fclose(logFile);			
			break;
		case ATHEADINGSPEEDMIXED:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atheadingspeedmixed>");
			fclose(logFile);			
			break;
		case ATOBSAVOIDNAIVE:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atobsavoidnaive>");
			fclose(logFile);			
			break;
                case ATOBSAVOID:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atobsavoid>");
			fclose(logFile);			
			break;
    		case ATSTEPPER:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atstepper>");
			fclose(logFile);			
			break; 
                default:
			break;
	}
        

        /*
        * Uninitialize the device
        */
        try {
            lmsp->Uninitialize();
        }

        catch(...) {
            cerr << "Uninitialize failed!" << endl;
        }
	
        exit(signum);	
}

void newPoint(double lat, double lon, double bearing, double distance, VECTOR *newP) {
    lat = lat * M_PI / 180;
    lon = lon * M_PI / 180; 
    double radius = 6371000;
    double quot = distance / radius;
    newP->lat = asin(sin(lat) * cos(quot) + cos(lat) * sin(quot) * cos(bearing));
    newP->lon = lon + atan2(sin(bearing) * sin(quot) * cos(lat), cos(quot) - sin(lat) * sin(newP->lat));
    newP->lat = newP->lat * 180 / M_PI;
    newP->lon = newP->lon * 180 / M_PI;
}

void *stepperInit(void *vargp)
{
    pthread_detach(pthread_self());
    int steps = 85 * 2;
    while (getPosition(maestro, getLimitswitch()))
    {
        usleep(1000);
        setTarget(maestro, getDirection(), 0);
	usleep(1000);
	myBoat.commandArgs[1]--;
        setTarget(maestro, getStepper(), 1500);
        usleep(2000);
	setTarget(maestro, getStepper(), 0);
        usleep(1000);
    }
    
    printf("Limit Switch Hit!\n");					
    
    setTarget(maestro, getDirection(), 1500);
    
    while (steps)
    {
	steps--;
        setTarget(maestro, getStepper(), 1500);
        usleep(2000);
	setTarget(maestro, getStepper(), 0);
        usleep(2000);
    }

    printf("Stepper In Position!\n");

    pthread_exit(0);
    return NULL;
}

void *stepperRotate(void *vargp)
{
    pthread_detach(pthread_self());
                        
    getPosition(maestro, getLimitswitch());

    usleep(1000);
    while (myBoat.commandArgs[1])
    {
        if (myBoat.commandArgs[0] < 1000)
        {
            if (!getPosition(maestro, getLimitswitch()))
	    {
                printf("Limit Switch Hit!\n");					
                break;
	    }
        }
        usleep(1000);
	setTarget(maestro, getDirection(), myBoat.commandArgs[0]);
        usleep(1000);
	myBoat.commandArgs[1]--;
	setTarget(maestro, getStepper(), 1500);
        usleep(2000);
	setTarget(maestro, getStepper(), 0);
        usleep(1000);
    }
    printf("Stepper in position!\n");

    pthread_exit(0);
    return NULL;
}
